﻿using System.Collections;
using System.Collections.Generic;

namespace TrueSync.Physics2D
{
    /// <summary>
    /// A contact edge is used to connect bodies and contacts together
    /// in a contact graph where each body is a node and each contact
    /// is an edge. A contact edge belongs to a doubly linked list
    /// maintained in each attached body. Each contact has two contact
    /// nodes, one for each attached body.
    /// </summary>
    public sealed class ContactEdge
    {
        /// <summary>
        /// The contact
        /// </summary>
        public Contact Contact;

        /// <summary>
        /// The next contact edge in the body's contact list
        /// </summary>
        public ContactEdge Next;

        /// <summary>
        /// Provides quick access to the other body attached.
        /// </summary>
        public Fixture Other;

        /// <summary>
        /// The previous contact edge in the body's contact list
        /// </summary>
        public ContactEdge Prev;
    }

    public class Contact
    {
        private enum ContactType
        {
            NotSupported,
            Circle,
            CircleAndRectagnle,
            Rectangle
        }

        private ContactType _type;

        private static ContactType[,] _registers = new[,]
                                                       {
                                                           {
                                                               ContactType.Circle,
                                                               ContactType.CircleAndRectagnle,
                                                           },
                                                           {
                                                               ContactType.CircleAndRectagnle,
                                                               ContactType.Rectangle,
                                                           },
                                                       };
        // Nodes for connecting bodies.
        internal ContactEdge _nodeA = new ContactEdge();
        internal ContactEdge _nodeB = new ContactEdge();

        public Fixture FixtureA;
        public Fixture FixtureB;

        /// <summary>
        /// Determines whether this contact is touching.
        /// </summary>
        /// <returns>
        /// 	<c>true</c> if this instance is touching; otherwise, <c>false</c>.
        /// </returns>
        public bool IsTouching { get; set; }

        private Contact(Fixture fA, Fixture fB)
        {
            Reset(fA, fB);
        }

        private void Reset(Fixture fA, Fixture fB)
        {
            IsTouching = false;

            FixtureA = fA;
            FixtureB = fB;

            _nodeA.Contact = null;
            _nodeA.Prev = null;
            _nodeA.Next = null;
            _nodeA.Other = null;

            _nodeB.Contact = null;
            _nodeB.Prev = null;
            _nodeB.Next = null;
            _nodeB.Other = null;
        }

        /// <summary>
        /// Update the contact manifold and touching status.
        /// Note: do not assume the fixture AABBs are overlapping or are valid.
        /// </summary>
        internal void Update()
        {
            if (FixtureA == null || FixtureB == null)
                return;

            bool touching = false;
            bool wasTouching = IsTouching;

            Shape shapeA = FixtureA.Shape;
            Shape shapeB = FixtureB.Shape;
            touching = Collision.TestOverlap(shapeA, shapeB, FixtureA.Xf, FixtureB.Xf);

            IsTouching = touching;

            if (wasTouching == false)
            {
                if (touching)
                {
                    // Report the collision to both participants. Track which ones returned true so we can
                    // later call OnSeparation if the contact is disabled for a different reason.
                    if (FixtureA.OnCollision != null)
                        FixtureA.OnCollision(FixtureA, FixtureB);

                    // Reverse the order of the reported fixtures. The first fixture is always the one that the
                    // user subscribed to.
                    if (FixtureB.OnCollision != null)
                        FixtureB.OnCollision(FixtureB, FixtureA);
                }
            }
        }

        internal static Contact Create(Fixture fixtureA, Fixture fixtureB)
        {
            ShapeType type1 = fixtureA.Shape.ShapeType;
            ShapeType type2 = fixtureB.Shape.ShapeType;

            Contact c;
            Queue<Contact> pool = fixtureA._world._contactPool;
            if (pool.Count > 0)
            {
                c = pool.Dequeue();
                if (type1 == ShapeType.Rectangle && type2 == ShapeType.Circle )
                {
                    c.Reset(fixtureB, fixtureA);
                }
                else
                {
                    c.Reset(fixtureA, fixtureB);
                }
            }
            else
            {
                if (type1 == ShapeType.Rectangle && type2 == ShapeType.Circle)
                {
                    c = new Contact(fixtureB, fixtureA);
                }
                else
                {
                    c = new Contact(fixtureA, fixtureB);
                }
            }

            c._type = _registers[(int)type1, (int)type2];

            return c;
        }

        internal void Destroy()
        {
            FixtureA._world._contactPool.Enqueue(this);
            Reset(null, null);
        }
    }
}
